#include "mpu6050_light_dmp.h"
#include "Driver/CLOCK/clock.h"
#include <stdio.h>
#include <math.h>

// 注意：此示例文件仅供参考，实际使用时应集成到主程序中
// 如果要单独编译此文件，需要确保包含必要的系统初始化代码

/**
 * @file mpu6050_light_dmp_example.c
 * @brief MPU6050轻量级DMP使用示例
 * 
 * 此文件展示了如何使用轻量级DMP功能：
 * 1. 初始化DMP
 * 2. 定期更新传感器数据
 * 3. 获取姿态信息
 * 4. 通过UART输出调试信息
 */

/****************************************************************************************************************/
/*                                        示例配置                                                             */
/****************************************************************************************************************/

// DMP配置参数
static MPU6050_DMP_Config_t dmp_config = {
    .beta = 0.1f,               // Madgwick算法参数
    .sample_rate = 100,         // 100Hz采样率
    .auto_calibrate = true,     // 启用自动校准
    .calibrate_samples = 1000   // 校准采样数
};

/****************************************************************************************************************/
/*                                        基础示例                                                             */
/****************************************************************************************************************/

/**
 * @brief 基础DMP使用示例（简化版，用于演示API使用方法）
 * @return 0成功，1失败
 * @note 此函数仅供参考，实际集成请参考主程序中的实现
 */
uint8_t MPU6050_LightDMP_BasicExample(void)
{
    float roll, pitch, yaw;
    uint32_t update_count = 0;
    uint32_t start_time = tick_ms;
    
    // 1. 初始化DMP
    if (MPU6050_LightDMP_Init(&dmp_config) != 0) {
        return 1;  // 初始化失败
    }
    
    // 2. 运行10秒钟的姿态检测示例
    while ((tick_ms - start_time) < 10000) {  // 运行10秒
        // 更新DMP数据（应在固定频率调用）
        if (MPU6050_LightDMP_Update() == 0) {
            // 获取姿态角度
            if (MPU6050_LightDMP_GetAttitude(&roll, &pitch, &yaw) == 0) {
                update_count++;
                
                // 每1秒记录一次数据（可通过调试器查看）
                if ((update_count % 100) == 0) {
                    // 在实际应用中，这里可以：
                    // 1. 通过OLED显示角度
                    // 2. 通过UART发送数据
                    // 3. 触发其他控制逻辑
                    // 当前仅作为示例，变量值可通过调试器观察
                    volatile float debug_roll = roll;
                    volatile float debug_pitch = pitch;
                    volatile float debug_yaw = yaw;
                    (void)debug_roll; (void)debug_pitch; (void)debug_yaw;  // 避免编译器警告
                }
            }
        }
        
        // 10ms延时，保持100Hz更新率
        mspm0_delay_ms(10);
    }
    
    return 0;
}

/****************************************************************************************************************/
/*                                        高级示例                                                             */
/****************************************************************************************************************/

/**
 * @brief 完整数据输出示例（简化版）
 * @return 0成功，1失败
 * @note 此函数演示如何获取完整的DMP数据结构
 */
uint8_t MPU6050_LightDMP_AdvancedExample(void)
{
    MPU6050_LightDMP_t dmp_data;
    uint32_t start_time = tick_ms;
    uint32_t sample_count = 0;
    
    // 初始化DMP（使用默认配置）
    if (MPU6050_LightDMP_Init(NULL) != 0) {
        return 1;  // 初始化失败
    }
    
    // 运行5秒钟的数据采集示例
    while ((tick_ms - start_time) < 5000) {  // 运行5秒
        // 更新DMP数据
        if (MPU6050_LightDMP_Update() == 0) {
            // 获取完整数据
            if (MPU6050_LightDMP_GetData(&dmp_data) == 0) {
                sample_count++;
                
                // 每2秒记录一次详细数据
                if ((sample_count % 200) == 0) {
                    // 在实际应用中，这些数据可以：
                    // 1. 存储到数组中进行分析
                    // 2. 通过通信接口发送出去
                    // 3. 用于姿态控制算法
                    volatile MPU6050_LightDMP_t debug_data = dmp_data;
                    (void)debug_data;  // 避免编译器警告
                }
            }
        }
        
        mspm0_delay_ms(10);  // 100Hz采样
    }
    
    return 0;
}

/****************************************************************************************************************/
/*                                        校准示例                                                             */
/****************************************************************************************************************/

/**
 * @brief 传感器校准示例（简化版）
 * @return 0成功，1失败
 * @note 此函数演示如何进行手动校准
 */
uint8_t MPU6050_LightDMP_CalibrationExample(void)
{
    // 自定义配置，禁用自动校准
    MPU6050_DMP_Config_t custom_config = {
        .beta = 0.1f,
        .sample_rate = 100,
        .auto_calibrate = false,  // 手动校准
        .calibrate_samples = 500
    };
    
    // 1. 初始化（不自动校准）
    if (MPU6050_LightDMP_Init(&custom_config) != 0) {
        return 1;  // 初始化失败
    }
    
    // 2. 等待2秒让传感器稳定
    mspm0_delay_ms(2000);
    
    // 3. 执行手动校准
    if (MPU6050_LightDMP_Calibrate(1500) != 0) {
        return 1;  // 校准失败
    }
    
    // 4. 测试校准结果 - 运行3秒
    uint32_t start_time = tick_ms;
    uint32_t test_count = 0;
    
    while ((tick_ms - start_time) < 3000) {  // 运行3秒
        if (MPU6050_LightDMP_Update() == 0) {
            test_count++;
            
            if ((test_count % 100) == 0) {  // 每秒记录一次
                MPU6050_LightDMP_t data;
                if (MPU6050_LightDMP_GetData(&data) == 0) {
                    // 校准后的数据可用于验证校准效果
                    // 正常情况下陀螺仪应接近0，加速度计Z轴应接近1g
                    volatile float gyro_drift_x = data.gyro_x;
                    volatile float gyro_drift_y = data.gyro_y;
                    volatile float gyro_drift_z = data.gyro_z;
                    (void)gyro_drift_x; (void)gyro_drift_y; (void)gyro_drift_z;
                }
            }
        }
        
        mspm0_delay_ms(10);
    }
    
    return 0;
}

/****************************************************************************************************************/
/*                                        应用示例                                                             */
/****************************************************************************************************************/

/**
 * @brief 简单的水平仪应用示例（简化版）
 * @return 0成功，1失败
 * @note 此函数演示如何检测水平状态
 */
uint8_t MPU6050_LightDMP_LevelMeterExample(void)
{
    float roll, pitch;
    const float LEVEL_THRESHOLD = 2.0f;  // 水平阈值(度)
    uint32_t start_time = tick_ms;
    uint32_t check_count = 0;
    uint32_t level_count = 0;  // 水平状态计数
    
    if (MPU6050_LightDMP_Init(NULL) != 0) {
        return 1;  // 初始化失败
    }
    
    // 运行10秒的水平检测示例
    while ((tick_ms - start_time) < 10000) {  // 运行10秒
        if (MPU6050_LightDMP_Update() == 0) {
            if (MPU6050_LightDMP_GetAttitude(&roll, &pitch, NULL) == 0) {
                check_count++;
                
                // 判断水平状态
                bool is_level_roll = (fabsf(roll) < LEVEL_THRESHOLD);
                bool is_level_pitch = (fabsf(pitch) < LEVEL_THRESHOLD);
                bool is_level = is_level_roll && is_level_pitch;
                
                if (is_level) {
                    level_count++;
                }
                
                // 每2秒记录一次状态
                if ((check_count % 100) == 0) {
                    // 在实际应用中，可以：
                    // 1. 通过LED指示水平状态
                    // 2. 通过蜂鸣器提醒
                    // 3. 在OLED上显示水平信息
                    volatile bool debug_is_level = is_level;
                    volatile float debug_roll = roll;
                    volatile float debug_pitch = pitch;
                    (void)debug_is_level; (void)debug_roll; (void)debug_pitch;
                }
            }
        }
        
        mspm0_delay_ms(50);  // 20Hz更新频率
    }
    
    // 计算水平率
    float level_rate = (float)level_count / check_count * 100.0f;
    volatile float debug_level_rate = level_rate;  // 可通过调试器查看
    (void)debug_level_rate;
    
    return 0;
}

/****************************************************************************************************************/
/*                                        主菜单                                                               */
/****************************************************************************************************************/

/**
 * @brief DMP示例主菜单
 */
/**
 * @brief DMP示例运行器（简化版）
 * @note 在实际项目中已集成到主程序，此函数作为独立测试用
 */
void MPU6050_LightDMP_ExampleMenu(void)
{
    // 简化版：依次运行所有示例
    MPU6050_LightDMP_BasicExample();
    mspm0_delay_ms(1000);
    
    MPU6050_LightDMP_AdvancedExample();
    mspm0_delay_ms(1000);
    
    MPU6050_LightDMP_CalibrationExample();
    mspm0_delay_ms(1000);
    
    MPU6050_LightDMP_LevelMeterExample();
}